#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <operator_dynamic/CompetitionAction.h>

class Competition
{
protected:
    ros::NodeHandle nh_;
    actionlib::SimpleActionServer<operator_dynamic::CompetitionAction> as_;
    std::string action_name_;
    operator_dynamic::CompetitionFeedback feedback_;
    operator_dynamic::CompetitionResult resutl_;

public:
    Competition(std::string name) :
            as_(nh_, name, boost::bind(&Competition::executeCB, this, _1), false),
            action_name_(name)
    {
        as_.start();
    }

    ~Competition() { }

    void executeCB(const operator_dynamic::CompetitionGoalConstPtr& goal)
    {
        ros::Rate r(0.9);
        bool success = true;


        feedback_.percent = 0;
        ROS_INFO("%s Executing, samples = %d", action_name_.c_str(), goal->samples);

        for(int i = 0; i <= goal->samples; i++)
        {
            if (as_.isPreemptRequested() || !ros::ok())
            {
                ROS_INFO("%s Preempted", action_name_.c_str());
                as_.setPreempted();
                success = false;

                break;
            }

            feedback_.percent = (float)i / (float)(goal->samples);

            ROS_INFO("percent = %f", feedback_.percent);
            as_.publishFeedback(feedback_);
            r.sleep();
        }

        if (success)
        {
            resutl_.name = "succeeded";
            ROS_INFO("%s Succeed", action_name_.c_str());
            as_.setSucceeded(resutl_);
        }
        else
        {
            resutl_.name = "failed";
            ROS_INFO("%s Failed", action_name_.c_str());
            as_.setAborted(resutl_, "mission failed");
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "Competition");
    Competition competition("Competition");
    ros::spin();
    return 0;
}